/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          :
 * Description        :
 ******************************************************************************
 * @attention
 *
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "main.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define T1 1.0f // 1s
#define VOLT_CAL 8.0f * Amp_Num_SCALE
#define VELOCITY_CAL 100.0f // rad/s
#define FILTER_COUNT 10
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
CalibrationStruct Calibration;
/* USER CODE END Variables */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
static void calibration_reset(CalibrationStruct *cal)
{
	cal->theta_ref = 0;
	cal->theta_diff = 0;
	cal->count = 0;
	cal->time = 0;
	cal->theta_start = 0;
	cal->sample_count = 0;
	cal->next_sample_time = 0;
	cal->switchover = 0;

	//	cal->pole_pairs = 0;
	//	cal->ezero = 0;
	//	cal->phase_order = 0;
	//	g_sys.RW.CaliPara.Para.pole_pairs = cal->pole_pairs;
	//	g_sys.RW.CaliPara.Para.phase_order = cal->phase_order;
	//	g_sys.RW.CaliPara.Para.ezero = cal->ezero;
}

void motor_calibration_enable(uint8_t pole_pairs)
{
	calibration_reset(&Calibration);
	g_sys.State = STATE_STOP;
	g_sys.Mode = MODE_CALI;
	g_sys.RW.CaliPara.Para.pole_pairs = pole_pairs;
	Brake_Control(1);
}

static void calibration_error(void)
{
	calibration_reset(&Calibration);
	g_sys.State = STATE_STOP;
}

void motor_encoder_calibration(EncoderStruct *encoder, ControllerStruct *controller, CalibrationStruct *cal)
{
	switch (cal->switchover)
	{
	case 0:
		cal->time = (float)cal->count * DT;
		cal->count++;
		if (cal->time < T1)
		{
			// order of phase must is zero
			g_sys.RW.CaliPara.Para.phase_order = 0;
			cal->phase_order = 0;

			cal->theta_ref = 0;
			cal->theta_start = encoder->angle_multi;
			cal->next_sample_time = cal->time;
			cal->sample_count = 0;
			controller->i_d_ref = VOLT_CAL;
			controller->i_q_ref = 0.0f;
			foc_commutate(cal->theta_ref);
			return;
		}
		else if (cal->time < (T1 + (TWO_PI * FILTER_COUNT / VELOCITY_CAL)))
		{
			cal->theta_ref += (VELOCITY_CAL * DT);
			foc_commutate(cal->theta_ref);
			if (cal->time > cal->next_sample_time)
			{
				cal->theta_diff += (encoder->angle_multi - cal->theta_start);
				cal->theta_start = encoder->angle_multi;
				cal->next_sample_time += (TWO_PI / VELOCITY_CAL);
			}
			return;
		}
		else
		{
			cal->theta_diff /= FILTER_COUNT;
			if (cal->theta_diff > 0.0628)
				cal->phase_order = 0;
			else if (cal->theta_diff < -0.0628)
				cal->phase_order = 1;
			else
			{
				// motor don't move,there is should a error **************************************
				calibration_error();
				return;
			}
			// maybe we can compare the intput pole-pairs with measured value
			// to determine the state of the magnet
			int pole_pairs = (int)roundf(TWO_PI / cal->theta_diff);
			cal->pole_pairs = abs(pole_pairs);
			// there is should a error **************************************
			if (cal->pole_pairs != g_sys.RW.CaliPara.Para.pole_pairs)
			{
				calibration_error();
				return;
			}
			g_sys.RW.CaliPara.Para.pole_pairs = cal->pole_pairs;
			g_sys.RW.CaliPara.Para.phase_order = cal->phase_order;

			calibration_reset(&Calibration);
			cal->switchover = 1;
		}

		break;
		/*---------------------------- calibrate zero-postion and cogging torque --------------------------*/
	case 1:
		cal->time = (float)cal->count * DT;
		cal->count++;

		if (cal->time < T1)
		{
			cal->theta_ref = 0;
			cal->theta_start = encoder->angle_multi;
			cal->next_sample_time = cal->time;
			cal->sample_count = 0;
			// controller->v_d = VOLT_CAL;
			// controller->v_q = 0.0f;
			controller->i_d_ref = VOLT_CAL;
			controller->i_q_ref = 0.0f;
			foc_commutate(cal->theta_ref);

			return;
		}
		else if (cal->time < (T1 + (TWO_PI * cal->pole_pairs / VELOCITY_CAL)))
		{
			cal->theta_ref += (VELOCITY_CAL * DT);
			foc_commutate(cal->theta_ref);

			if (cal->time > cal->next_sample_time)
			{
				int raw_ref = cal->theta_ref * (float)(IC_MU_OUTER_RANGE) / (TWO_PI * cal->pole_pairs);
				int error = encoder->raw - raw_ref;
				cal->error_arr[cal->sample_count] = error + IC_MU_OUTER_RANGE * (error < 0);
				// next sample time
				cal->next_sample_time += TWO_PI / (VELOCITY_CAL * LUT_COUNT);
				// sample count
				if (cal->sample_count == cal->pole_pairs * LUT_COUNT - 1)
					return;
				cal->sample_count++;
			}

			return;
		}
		else if (cal->time < (T1 + (2.0f * TWO_PI * cal->pole_pairs / VELOCITY_CAL)))
		{
			cal->theta_ref -= (VELOCITY_CAL * DT);
			foc_commutate(cal->theta_ref);
			
			if ((cal->time > cal->next_sample_time) && (cal->sample_count > 0))
			{
				int raw_ref = cal->theta_ref * (float)(IC_MU_OUTER_RANGE) / (TWO_PI * cal->pole_pairs);
				int error = encoder->raw - raw_ref;
				error = error + IC_MU_OUTER_RANGE * (error < 0);

				cal->error_arr[cal->sample_count] = (cal->error_arr[cal->sample_count] + error) / 2;
				// next sample time
				cal->next_sample_time += TWO_PI / (VELOCITY_CAL * LUT_COUNT);
				// sample count
				cal->sample_count--;
			}
			return;
		}
		else
		{
			// foc_reset
			cal->theta_ref = 0;
			controller->v_d = 0.0f;
			controller->v_q = 0.0f;
			g_sys.Mode = MODE_CALI;
			foc_commutate(cal->theta_ref);

			// calculate average offset
			int elec_zero_mean = 0;
			for (int i = 0; i < ((int)(cal->pole_pairs * LUT_COUNT)); i++)
			{
				elec_zero_mean += cal->error_arr[i];
			}
			cal->ezero = elec_zero_mean / (cal->pole_pairs * LUT_COUNT);

			int window = LUT_COUNT;
			int offset = (IC_MU_OUTER_RANGE - cal->error_arr[0]) * LUT_COUNT / IC_MU_OUTER_RANGE;
			for (int i = 0; i < LUT_COUNT; i++)
			{
				int move_avg = 0;
				for (int j = (-window) / 2; j < (window) / 2; j++)
				{
					int index = i * cal->pole_pairs + j;
					if (index < 0)
						index += cal->pole_pairs * LUT_COUNT;
					else if (index > (cal->pole_pairs * LUT_COUNT - 1))
						index -= cal->pole_pairs * LUT_COUNT;

					move_avg += cal->error_arr[index];
				}
				move_avg = move_avg / window;
				int lut_index = offset + i;
				if (lut_index > (LUT_COUNT - 1))
					lut_index -= LUT_COUNT;
				// cal->lut_arr[lut_index] = move_avg - cal->ezero;

				// calibration parameter to [RW.CaliPara]
				g_sys.RW.CaliPara.Para.cogging[lut_index] = move_avg - cal->ezero;
			}

			g_sys.RW.CaliPara.Para.ezero = cal->ezero;
			// g_sys.RW.CaliPara.Para.gear_ratio =

			// Rest calibration parameters
			calibration_reset(&Calibration);
			// Save calibration parameters
			UserPara_Calibration_Save();

			g_sys.State = STATE_STOP;
		}
		break;
		/*---------------------------- calibrate zero-postion and cogging torque --------------------------*/
	}
}

/* USER CODE END Application */
